#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 4
#define SENSORPIN A5
#define SPKRPIN 3
#define RX_1 5
#define TX_1 6
#define MAX_BUFFER_SIZE 64
#define TIME_OUT 10
#define MSG_RCVD 102
#define ID_REQUEST 103
#define SETUP_ID 104
#define LISTENING 0
#define CHANNEL_SET 1


SoftwareSerial board_port(RX_1,TX_1);

int active_channel;
int listen_count;
int state;
long prevTime;
int ledState;
uint8_t myID;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len;
int write_buf_len;


void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(byte val, byte *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return 1;
    }
    return 0;
}

void writebuffertoboards(uint8_t *buf, int buf_len){
    const uint8_t start_sig = 170;
    int temp_len = buf_len;
    uint8_t temp_buf[buf_len+1];
    while(temp_len > 0){
        temp_buf[temp_len] = buf[temp_len-1];
        temp_len--;
    }
    temp_buf[0] = start_sig;
    Serial.print("Writing");
    for(int i=0;i<buf_len+1;i++){
      Serial.print(", ");
      Serial.print(temp_buf[i], DEC);
    }
    Serial.println(" to serial the board network.");
    board_port.write(temp_buf, buf_len+1);
}

/* //Write message buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  int temp_len = buf_size;
  uint8_t temp_buf[buf_size+1];
  const uint8_t start_sig = 170;
  while(temp_len > 0){
    temp_buf[temp_len] = arr[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(i=0;i<buf_size+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  if(channel > 0 && channel < 5){
    mySerial[channel-1].write(temp_buf, buf_size+1);
  }else{
    Serial.println("Invalid channel selection for writebuffertoserial()");
    Serial.print("Error! cannot write to serial channel ");
    Serial.println(channel);
  }
} */


void send_msg_received(){
    uint8_t msg_rcvd = MSG_RCVD;
    for(int i=0;i<5;i++){
        writebuffertoboards(&msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption the our confirmation was received
}


void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT);
  pinMode(TX_1, OUTPUT);
  board_port.begin(TRXRATE);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  Serial.println("Generating random ID...");
  myID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  state = LISTENING;
  read_buf_len = 0;
  write_buf_len = 0;
  ledState = LOW;
  prevTime = millis();
}

void loop(){
  write_buf[0] = 2;
  write_buf[1] = 6;
  write_buf[2] = ID_REQUEST;
  int count = 0;
  int confirm_rcvd = 0;
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoboards(write_buf,3);
    delay(15);
    if(board_port.read() == 170){
        int cnt=0;
        while(board_port.available() > 0){
            read_buf[cnt] = board_port.read();
            cnt++;
        }
        read_buf_len = cnt;
        confirm_rcvd = 1;
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        board_port.flush();
        //while(board_port.available() > 0)board_port.read();
    }
    count++;
  }
  if(count >= TIME_OUT || read_buf_len <= 0){
    Serial.println("Error reading from boards.");
    Serial.println("No message received before TIME_OUT.");
  }else{
    Serial.println("Message received from boards:");
    for(int i=0;i<read_buf_len;i++){
        Serial.print(read_buf[i]);
        Serial.print(" ");
    }
    Serial.println("");
  }
  delay(1000);
}
